﻿// Copyright (c) 2024 刻BITTER
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.


#pragma once

#include <cstddef>
#include <cstdint>

#include "lipid.hpp"
#include "lipid_filter.hpp"
#include "lipid_io.hpp"
#include "lipid_io_impl.hpp"

// 与PID 数据的输入 / 输出有关
namespace lipid {


    /**
     * @brief PID 位置 - 速度双闭环控温
     *
     */
    class BicycleThermalPid {
       public:
        using GcIndexType = uint8_t;
        static constexpr GcIndexType PID_STAGE_COUNT = 2;

       private:
        PidCountCore _gc[PID_STAGE_COUNT];  // 两个PID 串联

        PidCountCore &_gc0 = _gc[0];
        PidCountCore &_gc1 = _gc[1];

        CachedSensor<float> *_main_feedback = nullptr;
        ISensor<float> *_gc1_feedback = nullptr;
        IActuator<float> *_out_to = nullptr;

        PidSignalDifferentiator _v_feedback{&_gc1, _main_feedback};

        float _gc0_out = 0;

        unsigned int _gc1_freq_times = 1;  // gc1 采样频率对gc0 的倍率，若倍率大于1，则gc1 计算多次后，gc0 才计算一次
        unsigned int _gc0_wait_counter = 0;

        float _last_gc0_feedback = 0;
        float _last_gc1_feedback = 0;

       public:
        void tune_gc0(float kp, float ki, float kd) {
            _gc0.tune(kp, ki, kd);
        }

        void tune_gc1(float kp, float ki, float kd) {
            _gc1.tune(kp, ki, kd);
        }

        void tune_gc1(float kp, float ki, float kd, float kv) {
            _gc1.tune(kp, ki, kd);
            _v_feedback.set_kd(kv);
        }

        void set_sample_time(float gc1_sample_time, unsigned int gc1_freq_times) {
            if (gc1_freq_times < 1) {
                gc1_freq_times = 1;
            }

            float gc0_sample_time = gc1_sample_time * static_cast<float>(gc1_freq_times);
            _gc1_freq_times = gc1_freq_times;
            _gc0.set_sample_time(gc0_sample_time);
            _gc1.set_sample_time(gc1_sample_time);
        }

        float forward_pass(float set_point, float gc0_feedback, float gc1_feedback) {
            _gc0_wait_counter++;
            if (_gc0_wait_counter == _gc1_freq_times) {
                _gc0_out = _gc0.count(set_point, gc0_feedback);
                _gc0_wait_counter = 0;
            }

            float gc1_out = _gc1.count(_gc0_out, gc1_feedback);
            return gc1_out;
        }

        float count(float set_point) {
            _IOXX_ASSERT_WITH_MESSAGE(_out_to != nullptr, "");
            _IOXX_ASSERT_WITH_MESSAGE(_main_feedback != nullptr, "");
            _IOXX_ASSERT_WITH_MESSAGE(_gc1_feedback != nullptr, "");

            _last_gc0_feedback = _main_feedback->update();
            float gc1f = _gc1_feedback->update();
            float out = forward_pass(set_point, _last_gc0_feedback, gc1f);
            _out_to->set(out);
            _gc0.count_end();
            _gc1.count_end();
            return out;
        }

        float get_main_feedback() const {
            return _last_gc0_feedback;
        }

        float get_gc1_feedback() const {
            return _last_gc1_feedback;
        }

        const PidCountCore &gc0() const {
            return _gc0;
        }

        const PidCountCore &gc1() const {
            return _gc1;
        }

        PidCountCore &gc0() {
            return _gc0;
        }

        PidCountCore &gc1() {
            return _gc1;
        }

        template <GcIndexType Which>
        PidCountCore &gc() {
            static_assert(Which < PID_STAGE_COUNT, "Index Out Of Bound.");
            return _gc[Which];
        }

        template <GcIndexType Which>
        const PidCountCore &gc() const {
            static_assert(Which < PID_STAGE_COUNT, "Index Out Of Bound.");
            return _gc[Which];
        }

        void set_as_v_feedback(CachedSensor<float> *main_feedback, IActuator<float> *out_to) {
            _main_feedback = main_feedback;
            _out_to = out_to;
            _v_feedback.set_source(main_feedback);
            _gc1_feedback = &_v_feedback;

        }


        // void reset() {
        //     for (int i = 0; i < PidStageCount; ++i) {
        //         _gc[i].reset();
        //     }
        //     _gc0_out = 0;
        //     _gc0_wait_counter = 0;
        //     _last_main_feedback = 0;
        // }
    };
}  // namespace lipid